//////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008-2010, Shane J. M. Liesegang
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions are met:
// 
//     * Redistributions of source code must retain the above copyright 
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright 
//       notice, this list of conditions and the following disclaimer in the 
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the copyright holder nor the names of any 
//       contributors may be used to endorse or promote products derived from 
//       this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////

#include "SpatialGraph.h"

#include "TextRendering.h"
#include "World.h"
#include "Ray2.h"
#include "DrawUtil.h"
#include "StringUtil.h"
#include "MathUtil.h"

#include "../_Box2D/Box2D.h"

void SpatialGraphKDNode::Render()
{
        if( HasChildren() )
        {
                LHC->Render();
                RHC->Render();
                return;
        }

        if( theSpatialGraph.GetDrawBlocked() )
        {
                if( bBlocked )
                {
                        glColor4f(1,0,0,0.25f);
                        BBox.RenderBox();
                }
        }

        if( theSpatialGraph.GetDrawBounds() )
        {
                glColor4f(0,0,0,1.f);
                BBox.RenderOutline();
        }


        Vector2 centroid = BBox.Centroid();

        if( theSpatialGraph.GetDrawNodeIndex() )
        {
                Vector2 screenCenter = MathUtil::WorldToScreen( centroid.X, centroid.Y );
                //Print some vals
                glColor3f(0,1.f,1.f);
                DrawGameText( IntToString(Index), "ConsoleSmall", (int)screenCenter.X, (int)screenCenter.Y );
        }

        if( theSpatialGraph.GetDrawGraph() && !bBlocked )
        {
                glColor3f(1.f,0.f,0.f);

                for( unsigned int i = 0; i < Neighbors.size(); i++ )
                {
                        if( Neighbors[i]->bBlocked || !NeighborLOS[i] )
                                continue;
                        //Draw lines
                        glBegin(GL_LINES);
                        //draw centroid to centroid half way point
                        Vector2 neighbor = Neighbors[i]->BBox.Centroid();
                        neighbor = centroid + ((neighbor - centroid) * 0.6f);

                        glVertex2f(centroid.X, centroid.Y);
                        glVertex2f(neighbor.X, neighbor.Y);

                        glEnd();
                }
        }

        if( theSpatialGraph.GetDrawGridPoints() )
        {
                glColor3f(1.f,0.f,0.f);
                Vector2List gridPoints;
                int xPoints, yPoints;
                GetGridPoints(gridPoints, xPoints, yPoints );

                for( unsigned int i = 0; i < gridPoints.size(); i++ )
                {
                        DrawPoint( gridPoints[i], Tree->GetSmallestDimensions().X * 0.15f );
                }

        }
}

void SpatialGraphKDNode::GetGridPoints( Vector2List& points, int& xPoints, int& yPoints )
{
        xPoints = 0;
        yPoints = 0;

        Vector2 vSmallestDimensions = Tree->GetSmallestDimensions();
        Vector2 vMyBoxDimensions = BBox.Max - BBox.Min;

        /*
        if( vSmallestDimensions == vMyBoxDimensions )
        {
                xPoints = 1;
                yPoints = 1;
                points.push_back( BBox.Centroid() );
                return;
        }
        */

        xPoints = static_cast<int>(vMyBoxDimensions.X / vSmallestDimensions.X);
        yPoints = static_cast<int>(vMyBoxDimensions.Y / vSmallestDimensions.Y);
        points.reserve(xPoints*yPoints);

        Vector2 vBottomLeftStartBox( BBox.Min.X, BBox.Max.Y - vSmallestDimensions.Y );

        BoundingBox startBox( vBottomLeftStartBox, vBottomLeftStartBox + vSmallestDimensions);

        BoundingBox checkBox( startBox );

        for( int yDim = 0; yDim < yPoints; yDim++ )
        {
                for( int xDim = 0; xDim < xPoints; xDim++ )
                {
                        points.push_back( checkBox.Centroid() );

                        checkBox.Min.X += vSmallestDimensions.X;
                        checkBox.Max.X += vSmallestDimensions.X;
                }

				checkBox.Min.X = startBox.Min.X;
                checkBox.Max.X = startBox.Max.X;

                checkBox.Min.Y -= vSmallestDimensions.Y;
                checkBox.Max.Y -= vSmallestDimensions.Y;
        }

}


SpatialGraph::SpatialGraph(float entityWidth, const BoundingBox& startBox )
{
        _entityWidth = entityWidth;
        float maxDimension = MathUtil::Max( startBox.Max.Y - startBox.Min.Y, startBox.Max.X - startBox.Min.X );
        int depth = 0;
        while( maxDimension > _entityWidth )
        {
                maxDimension /= 2.0f;
                depth += 2;
        }
        _depth = MathUtil::Max(depth,1);
        if( _depth > 24 )
                _depth = 24;

        int depthMask = ~(0xFFFFFFFF << _depth);

        _dirMasks[0] = 0x1;
        _dirMasks[1] = 0x2;
        _dirMasks[2] = 0xaaaaaaaa & depthMask;
        _dirMasks[3] = _dirMasks[2] >> 1;

        _root = CreateTree(_depth+1, startBox, NULL);

        //Get smallest dimension
        _smallestDimensions = startBox.Max - startBox.Min;
        for( int i = 0; i < _depth; i++ )
        {
                if( i % 2 )
                        _smallestDimensions.Y *= 0.5f;
                else
                        _smallestDimensions.X *= 0.5f;
        }

        ComputeNeighbors( _root );
        ValidateNeighbors( _root );
}

SpatialGraph::~SpatialGraph()
{
        DeleteNode( _root );
}

void SpatialGraph::DeleteNode( SpatialGraphKDNode* pNode )
{
        if( pNode == NULL )
                return;

        DeleteNode( pNode->LHC );
        DeleteNode( pNode->RHC );

        delete pNode;
}



SpatialGraphKDNode* SpatialGraph::FindNode(SpatialGraphKDNode* node, const BoundingBox& bbox)
{
        if (node == NULL)
                return NULL;
        //check if this bbox fits entirely within our node
        if (node->BBox.Contains(bbox) == Within)
        {
                //Check our children
                SpatialGraphKDNode* retVal = FindNode(node->LHC, bbox );
                if (retVal != NULL)
                        return retVal;
                retVal = FindNode(node->RHC, bbox);
                if( retVal != NULL )
                        return retVal;

                //otherwise, return ourselves
                return node;

        }
        return NULL;
}

SpatialGraphKDNode* SpatialGraph::FindNode(SpatialGraphKDNode* node, const Vector2& point)
{
        if (node == NULL)
                return NULL;
        //check if this bbox fits entirely within our node
        if (node->BBox.Contains(point))
        {
                //Check our children
                SpatialGraphKDNode* retVal = FindNode(node->LHC, point );
                if (retVal != NULL)
                        return retVal;
                retVal = FindNode(node->RHC, point);
                if( retVal != NULL )
                        return retVal;

                //otherwise, return ourselves
                return node;

        }
        return NULL;
}


SpatialGraphKDNode* SpatialGraph::FindNode(const BoundingBox& bbox)
{
        return FindNode(_root, bbox);
}

SpatialGraphKDNode* SpatialGraph::FindNode(const Vector2& point)
{
        return FindNode(_root, point);
}

void SpatialGraph::Render()
{
        bool bDrawAny = theSpatialGraph.GetDrawBounds() || theSpatialGraph.GetDrawGridPoints() || theSpatialGraph.GetDrawGraph() || theSpatialGraph.GetDrawBlocked();

        if( bDrawAny && _root)
        {
                _root->Render();
        }
}

bool IsBlocked( const BoundingBox& bbox )
{
        const int maxShapeCount = 1024;
        b2AABB physBounds;
        physBounds.lowerBound = b2Vec2( bbox.Min.X, bbox.Min.Y ); 
        physBounds.upperBound = b2Vec2( bbox.Max.X, bbox.Max.Y );
        static b2Shape* tempShapes[maxShapeCount];

        int numBroadphase = theWorld.GetPhysicsWorld().Query( physBounds, tempShapes, maxShapeCount );

        //No bodies here
        if( numBroadphase == 0 )
                return false;

        b2PolygonDef shapeBoundsDef;
        shapeBoundsDef.vertexCount = 4;
        shapeBoundsDef.vertices[0].Set( physBounds.lowerBound.x, physBounds.lowerBound.y );
        shapeBoundsDef.vertices[1].Set( physBounds.upperBound.x, physBounds.lowerBound.y );
        shapeBoundsDef.vertices[2].Set( physBounds.upperBound.x, physBounds.upperBound.y );
        shapeBoundsDef.vertices[3].Set( physBounds.lowerBound.x, physBounds.upperBound.y );

        b2BodyDef fakeBodyDef;
        //b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents);
        fakeBodyDef.position.Set(0.0f, 0.0f );
        b2Body* fakeBody = theWorld.GetPhysicsWorld().CreateBody(&fakeBodyDef);
        b2Shape* shapeBounds = fakeBody->CreateShape( &shapeBoundsDef );

        for( int i = 0; i < numBroadphase; i++ )
        {
                b2Shape* pSh = tempShapes[i];
                if( pSh->GetType() == e_polygonShape  )
                {
                        b2PolygonShape* pPolyShape = (b2PolygonShape*)pSh;

                        b2Manifold m0;
                        b2CollidePolygons(&m0, (b2PolygonShape*)shapeBounds, fakeBody->GetXForm(), pPolyShape, pPolyShape->GetBody()->GetXForm());

                        if( m0.pointCount > 0 )
                        {
                                theWorld.GetPhysicsWorld().DestroyBody(fakeBody);
                                return true;    
                        }
                }
                else if( pSh->GetType() == e_circleShape )
                {
                        b2CircleShape* pCircleShape = (b2CircleShape*)pSh;
                        b2Manifold m0;
                        b2CollidePolygonAndCircle( &m0, (b2PolygonShape*)shapeBounds, fakeBody->GetXForm(), pCircleShape, pCircleShape->GetBody()->GetXForm());
                        if( m0.pointCount > 0 )
                        {
                                theWorld.GetPhysicsWorld().DestroyBody(fakeBody);
                                return true;                            
                        }
                }
        }

        theWorld.GetPhysicsWorld().DestroyBody(fakeBody);

        return false;

}

SpatialGraphKDNode* SpatialGraph::CreateTree(int depth, const BoundingBox& bbox, SpatialGraphKDNode* parent, int index)
{
        SpatialGraphKDNode* node = new SpatialGraphKDNode(bbox, parent);
        node->Tree = this;
        node->bBlocked = false;

        //query physics to see if we're blocked
        node->bBlocked = IsBlocked( bbox );

        //Calculate my index
        if( parent )
        {
                node->Index = index;
        }
        else
        {
                node->Index = 0;
        }

        //Bail out if we reach max depth
        depth--;
        node->Depth = _depth - depth;
        if (depth > 0 && node->bBlocked )
        {
                BoundingBox LHSbbox, RHSbbox;
                MathUtil::SplitBoundingBox( bbox, depth % 2 ? MathUtil::AA_X : MathUtil::AA_Y, LHSbbox, RHSbbox );
                node->LHC = CreateTree(depth, LHSbbox, node, node->Index << 1);
                node->RHC = CreateTree(depth, RHSbbox, node, (node->Index << 1) + 1);

                int iMask = ~(0xFFFFFFFF << depth );
                //If I have children, pad my index
                node->Index = (node->Index << depth) | iMask;

                //If all my children are blocked, then destroy my children
                if( IsFullyBlocked(node) )
                {
                        DeleteNode( node->LHC );
                        node->LHC = NULL;
                        DeleteNode( node->RHC );
                        node->RHC = NULL;
                }
        }

        return node;
}

bool SpatialGraph::IsFullyBlocked( SpatialGraphKDNode* node )
{
        if( node == NULL )
                return true;

        return node->bBlocked && IsFullyBlocked(node->LHC) && IsFullyBlocked(node->RHC);
}

bool QuickContainsNode( hashmap_ns::hash_map<SpatialGraphKDNode*, int>& NodeList, SpatialGraphKDNode* pNode )
{
        hashmap_ns::hash_map<SpatialGraphKDNode*, int>::iterator itr = NodeList.find( pNode );
        return itr != NodeList.end();
}

void NudgePointOnPlane( const BoundingBox& BBox, Vector2& vPointOnPlane )
{
        //Get off the x planes
        if( vPointOnPlane.X == BBox.Min.X )
        {
                vPointOnPlane.X += MathUtil::Epsilon;
        }
        else if( vPointOnPlane.X == BBox.Max.X )
        {
                vPointOnPlane.X -= MathUtil::Epsilon;
        }

        //Get off the Y planes
        if( vPointOnPlane.Y == BBox.Min.Y )
        {
                vPointOnPlane.Y += MathUtil::Epsilon;
        }
        else if( vPointOnPlane.Y == BBox.Max.Y )
        {
                vPointOnPlane.Y -= MathUtil::Epsilon;
        }
}


bool SpatialGraph::CanGo( const Vector2& vFrom, const Vector2 vTo )
{

        //Get source cell
        SpatialGraphKDNode* pSourceNode = FindNode( vFrom );
        if( pSourceNode == NULL || pSourceNode->bBlocked)
                return false;

        SpatialGraphKDNode* pDestNode = FindNode( vTo );
        if( pDestNode == NULL || pDestNode->bBlocked)
                return false;

        return CanGoInternal( vFrom, vTo, pSourceNode, pDestNode );
}

bool SpatialGraph::CanGoInternal( const Vector2& vFrom, const Vector2 vTo, SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode )
{
        //If source is dest, we definitely can go (we're both within bounding box)
        if( pSourceNode == pDestNode )
                return true;

        Vector2 vUseFrom = vFrom;
        Vector2 vUseTo = vTo;

        NudgePointOnPlane( pSourceNode->BBox, vUseFrom );
        NudgePointOnPlane( pDestNode->BBox, vUseTo );

        Ray2 ray = Ray2::CreateRayFromTo( vUseFrom, vUseTo );

        hashmap_ns::hash_map<SpatialGraphKDNode*, int> NodeList;
        SpatialGraphKDNode* pCurrent = pSourceNode;

        while( true )
        {
                //Mark current as visited
                NodeList[pCurrent] = 1;
                SpatialGraphKDNode* pNearestNeighbor = NULL;
                float fNearestNeighborDistance = MathUtil::MaxFloat;
                //iterate over currents neighbors to see if they intersect the ray
                for( unsigned int i = 0; i < pCurrent->Neighbors.size(); i++ )
                {
                        SpatialGraphKDNode* pNeighbor = pCurrent->Neighbors[i];

                        //Ignore neighbors we've already visited
                        if( QuickContainsNode( NodeList, pNeighbor) )
                                continue;

                        float fDistanceAlongRay;
                        if( pNeighbor->BBox.Intersects( ray, fDistanceAlongRay ) )
                        {
                                if( fDistanceAlongRay < fNearestNeighborDistance )
                                {
                                        fNearestNeighborDistance = fDistanceAlongRay;
                                        pNearestNeighbor = pNeighbor;
                                }
                        }
                }

                //If we couldn't find a nearest neighbor, or the neighbor is blocked bail out
                if( pNearestNeighbor == NULL || pNearestNeighbor->bBlocked )
                        break;

                //If the nearest neighbor is our destination, we found it!
                if( pNearestNeighbor == pDestNode )
                        return true;

                //otherwise, check our neighbor
                pCurrent = pNearestNeighbor;
        }

        return false;
}

bool SpatialGraph::CanGoNodeToNode( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode )
{
        return CanGoInternal( pSourceNode->BBox.Centroid(), pDestNode->BBox.Centroid(), pSourceNode, pDestNode );
}


void SpatialGraph::AddNeighbor( SpatialGraphKDNode* node, const Vector2& pos )
{
        SpatialGraphKDNode* pNeighbor = node->Tree->FindNode( pos );
        if( pNeighbor )
        {
                //Add unique
                for( unsigned int i = 0; i < node->Neighbors.size(); i++ )
                {
                        if( node->Neighbors[i] == pNeighbor )
                                return;
                }

                node->Neighbors.push_back( pNeighbor );
                node->NeighborLOS.push_back(true);
        }

}

int GetColumnMajorIndex( int wantX, int wantY, int maxX )
{
        //Get column
        return (wantY * maxX) + wantX;
}

void SpatialGraph::ComputeNeighbors( SpatialGraphKDNode* node )
{
        if( node->HasChildren() )
        {
                ComputeNeighbors(node->LHC);
                ComputeNeighbors(node->RHC);
                return;
        }

        Vector2 checkN = Vector2::UnitY * _smallestDimensions.Y;
        Vector2 checkS = Vector2::UnitY * -_smallestDimensions.Y;
        Vector2 checkE = Vector2::UnitX * _smallestDimensions.X;
        Vector2 checkW = Vector2::UnitX * -_smallestDimensions.X;

        Vector2 centroid = node->BBox.Centroid();

        Vector2List gridPoints;
        int xPoints, yPoints;
        node->GetGridPoints(gridPoints, xPoints, yPoints );

        //Check north neighbors
        for( int i = 0; i < xPoints; i++ )
        {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,0,xPoints)] + checkN );
        }

        //Check south neighbors
        for( int i = 0; i < xPoints; i++ )
        {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,yPoints-1,xPoints)] + checkS );
        }

        //Check east neighbors
        for( int i = 0; i < yPoints; i++ )
        {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(xPoints-1,i,xPoints)] + checkE );
        }

        //Check west neighbors
        for( int i = 0; i < yPoints; i++ )
        {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(0,i,xPoints)] + checkW );
        }

}

void SpatialGraph::ValidateNeighbors( SpatialGraphKDNode* node )
{
        if( node->HasChildren() )
        {
                ValidateNeighbors(node->LHC);
                ValidateNeighbors(node->RHC);
                return;
        }
        //Validate neighbors
        for (unsigned int i = 0; i < node->Neighbors.size(); i++ )
        {
                //Todo, incorporate entity width
                if( !CanGoNodeToNode( node, node->Neighbors[i]) )
                {
                        node->NeighborLOS[i] = false;
                }
        }
}


SpatialGraphManager* SpatialGraphManager::s_SpatialGraphManager = NULL;
SpatialGraphManager & SpatialGraphManager::GetInstance()
{
        if( s_SpatialGraphManager == NULL)
        {
                s_SpatialGraphManager = new SpatialGraphManager();
                s_SpatialGraphManager->Initialize();
        }
        return *s_SpatialGraphManager;
}

SpatialGraphManager::SpatialGraphManager():
_spatialGraph(NULL),
_drawBounds(false),
_drawBlocked(false),
_drawGridPoints(false),
_drawGraph(false),
_drawNodeIndex(false)
{

}

SpatialGraphManager::~SpatialGraphManager()
{

}

void SpatialGraphManager::CreateGraph( float entityWidth, const BoundingBox& bounds )
{
        if( _spatialGraph != NULL )
                delete _spatialGraph;

        _spatialGraph = new SpatialGraph( entityWidth, bounds );
}

//Vector2List s_tempPath;
void SpatialGraphManager::Render()
{
        if( _spatialGraph  )
                _spatialGraph->Render();
}

bool ContainsNode( SpatialGraphNeighborList& path, const SpatialGraphKDNode* node)
{
        for( SpatialGraphNeighborList::iterator itr = path.begin(); itr != path.end(); itr++ )
        {
                if( (*itr) == node )
                        return true;
        }

        return false;
}

#include "stlastar.h"

struct SearchInterface
{
        SpatialGraphKDNode* pNode;

        SearchInterface( SpatialGraphKDNode* node )
        {
                pNode = node;
        }

        SearchInterface()
        {
                pNode = NULL;
        }

        ~SearchInterface()
        {
        }

        float GoalDistanceEstimate( const SearchInterface& goal )
        {
                return GetCost( goal );
        }

        bool IsGoal( const SearchInterface& goal )
        {
                return IsSameState(goal);
        }

        bool IsSameState( const SearchInterface& goal )
        {
                return pNode == goal.pNode;
        }

        bool GetSuccessors( AStarSearch<SearchInterface>* pSearch, SearchInterface* pParent )
        {
                for( unsigned int i = 0; i < pNode->Neighbors.size(); i++ )
                {
                        SpatialGraphKDNode* successor = pNode->Neighbors[i];
                        if( (!pParent || pParent->pNode != successor) && !successor->bBlocked && pNode->NeighborLOS[i])
                        {
                                SearchInterface si(successor);
                                if( !pSearch->AddSuccessor(si) )
                                        return false;
                        }
                }
                return true;
        }

        float GetCost( const SearchInterface& successor )
        {
                return Vector2::Distance( pNode->BBox.Centroid(), successor.pNode->BBox.Centroid() );
        }
};

bool ComputeAStar( SpatialGraphKDNode* pSourceNode, SpatialGraphKDNode* pDestNode, Vector2List& path )
{
        AStarSearch<SearchInterface> search;
        SearchInterface pStart(pSourceNode);
        SearchInterface pEnd(pDestNode);
        
        search.SetStartAndGoalStates( pStart, pEnd );

        while( AStarSearch<SearchInterface>::SEARCH_STATE_SEARCHING == search.SearchStep() )
        {

        }

        int curState = search.GetState();
        if( curState == AStarSearch<SearchInterface>::SEARCH_STATE_SUCCEEDED )
        {
                //Get path
                for( SearchInterface* pCur = search.GetSolutionStart(); pCur != NULL; pCur = search.GetSolutionNext() )
                {
                        path.push_back( pCur->pNode->BBox.Centroid() );
                }
                search.FreeSolutionNodes();
                return true;
        }

        return false;
}

bool SpatialGraphManager::GetPath( const Vector2& source, const Vector2& dest, Vector2List& path )
{
        if( _spatialGraph == NULL )
                return false;

        //Get source cell
        SpatialGraphKDNode* pSourceNode = _spatialGraph->FindNode( source );
        if( pSourceNode == NULL )
                return false;

        SpatialGraphKDNode* pDestNode = _spatialGraph->FindNode( dest );
        if( pDestNode == NULL )
                return false;

        path.push_back(source);
        if( pSourceNode == pDestNode )
        {
                path.push_back(dest);
                return true;
        }

        //Compute A*
        bool retVal = ComputeAStar(pSourceNode, pDestNode, path);
        if( retVal == false )
        {
                path.clear();
                return false;
        }

        //otherwise, put dest on the pathlist
        path.push_back(dest);

        return true;
}

bool SpatialGraphManager::CanGo( const Vector2& from, const Vector2 to )
{

        if( _spatialGraph == NULL )
                return false;

        return _spatialGraph->CanGo( from, to );

}

bool SpatialGraphManager::IsInPathableSpace( const Vector2& point )
{
        return CanGo(point,point);
}

bool SpatialGraphManager::FindNearestNonBlocked( const Vector2& fromPoint, Vector2& goTo )
{
        if( _spatialGraph == NULL )
                return false;

        SpatialGraphKDNode* pCurrentNode = _spatialGraph->FindNode( fromPoint );
        if( pCurrentNode == NULL )
                return false;

        float fMinDistance = MathUtil::MaxFloat;
        SpatialGraphKDNode* pNearestNeighbor = NULL;
        //otherwise, iterate over neighbors to find a non-blocked
        for( unsigned int i = 0; i < pCurrentNode->Neighbors.size(); i++ )
        {
                SpatialGraphKDNode* pNeighbor = pCurrentNode->Neighbors[i];
                if( pNeighbor->bBlocked )
                        continue;

                Vector2 vDir = pNeighbor->BBox.Centroid() - fromPoint;
                Ray2 ray( fromPoint, Vector2::Normalize(vDir) );

                float distanceToBBox;
                if( pNeighbor->BBox.Intersects(ray, distanceToBBox) )
                {
                        if( distanceToBBox < fMinDistance )
                        {
                                fMinDistance = distanceToBBox;
                                pNearestNeighbor = pNeighbor;
                        }
                }
        }

        if( pNearestNeighbor != NULL )
        {
                goTo = pNearestNeighbor->BBox.Centroid();
                return true;
        }

        return false;
}


void SpatialGraphManager::Initialize()
{
}


void SpatialGraphManager::EnableDrawBounds(bool enable)
{
        _drawBounds = enable;
}

const bool SpatialGraphManager::ToggleDrawBounds()
{
        EnableDrawBounds( !GetDrawBounds() );
        return GetDrawBounds();
}

const bool SpatialGraphManager::GetDrawBounds()
{
        return _drawBounds;
}

void SpatialGraphManager::EnableDrawBlocked(bool enable)
{
        _drawBlocked = enable;
}

const bool SpatialGraphManager::ToggleDrawBlocked()
{
        EnableDrawBlocked( !GetDrawBlocked() );
        return GetDrawBlocked();
}

const bool SpatialGraphManager::GetDrawBlocked()
{
        return _drawBlocked;
}

void SpatialGraphManager::EnableDrawGridPoints(bool enable)
{
        _drawGridPoints = enable;
}

const bool SpatialGraphManager::ToggleDrawGridPoints()
{
        EnableDrawGridPoints( !GetDrawGridPoints() );
        return GetDrawGridPoints();
}

const bool SpatialGraphManager::GetDrawGridPoints()
{
        return _drawGridPoints;
}

void SpatialGraphManager::EnableDrawGraph(bool enable)
{
        _drawGraph = enable;
}

const bool SpatialGraphManager::ToggleDrawGraph()
{
        EnableDrawGraph( !GetDrawGraph() );
        return GetDrawGraph();
}

const bool SpatialGraphManager::GetDrawGraph()
{
        return _drawGraph;
}

void SpatialGraphManager::EnableDrawNodeIndex(bool enable)
{
        _drawNodeIndex = enable;
}

const bool SpatialGraphManager::ToggleDrawNodeIndex()
{
        EnableDrawNodeIndex( !GetDrawNodeIndex() );
        return GetDrawNodeIndex();
}

const bool SpatialGraphManager::GetDrawNodeIndex()
{
        return _drawNodeIndex;
}